#ifndef __SENSORS_LAYER_H__
#define __SENSORS_LAYER_H__

#include "perception/costmap_2d/common/layered_costmap.h"
#include "observation_buffer.h"
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>

#include <unordered_map>
using namespace std;    

namespace costmap_2d{

class SensorsLayer : public Layer, public Costmap2D{
    public:
        SensorsLayer();
        virtual ~SensorsLayer();

        virtual void onInitialize();
        virtual void matchSize();
        virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                                  double* max_x, double* max_y);
        virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);

        void raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
            double* max_x, double* max_y);
        void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range,
            double* min_x, double* min_y, double* max_x, double* max_y);
      
        void setMap(const nav_msgs::OccupancyGridConstPtr& new_map) {};
        void createBuffer(boost::shared_ptr<costmap_2d::ObservationBuffer> buffer, string topic_name);
        void addPointCloud2(sensor_msgs::PointCloud2 cloud, string topic_name);
        
    private:
        bool rolling_window_;     
        std::string global_frame_;
        unordered_map<string, boost::shared_ptr<costmap_2d::ObservationBuffer>> observation_buffers_;
};

}   // namespace costmap_2d
    

#endif
